import ROSLIB from "roslib";
// import { mapsTopic } from "../js/topic";

// Ros连接
var ros = null;
// Map所在的画布
var viewer = null;
// 画布对象
var gridClient = null;
// 放大倍数的倒数，用于控制地图缩放比例：推荐范围0.1（10倍）~1（原图），不能为0
var scale = 1.0;
// 首次检测flag，用于判断是否为首次创建地图。
var check = false;
// 网格对象
var grid = null;

/**
 * 在目标元素创建画布
 * @param { Number } width 画布宽度
 * @param { Number} height 画布长度
 * @param { string } id 创建画布的元素的id,最好保持默认
 */
function createView(width = 500, height = 500, id = "map") {
  // 创建2D导航画布
  viewer = new window.ROS2D.Viewer({
    divID: id,
    width: width,
    height: height
  });
}

/**
 * 为当前viewer绘制地图,创建画布对象，拖拽事件生成和机器人箭头生成
 * @param { Object } link 连接对象，可传入ROSLIB.Ros对象或连接地址作为连接
 * @param { Object } self 可选，作为回调函数的参数，可传入组件环境
 * @param { Function } callback 回调函数，将在每次地图发生变动（监听事件'change'）之后回调
 */
function draw(link, self, callback) {
  // Connect to ROS.
  if (link.roshandler != null) {
    console.log("ros in draw setted by param'roshandler'");
    ros = link.roshandler;
  } else {
    console.log("ros in draw setted relying store :" + link.url);
    ros = new ROSLIB.Ros({
      url: link.url
    });
  }
  // 监听连接
  ros.on("connection", function () {
    console.log("ROS in draw: Connected to websocket server.");
  });

  ros.on("error", function (error) {
    console.log("ROS in draw: Error connecting to websocket server: ", error);
  });

  ros.on("close", function () {
    console.log("ROS in draw: Connection to websocket server closed.");
  });

  // mapsTopic.subscribe(function () {
  //   console.log("mapsTopic subscribed");
  // });

  gridClient = new window.ROS2D.OccupancyGridClient({
    ros: ros,
    rootObject: viewer.scene,
    continuous: true
  });
  // viewer.shift(gridClient.currentGrid.pose.position.x, gridClient.currentGrid.pose.position.y);

  // 监听地图变动事件
  gridClient.on("change", function () {
    console.log("set scale: " + scale);
    // 根据scale参数调整地图大小
    viewer.scaleToDimensions(
      gridClient.currentGrid.width * scale,
      gridClient.currentGrid.height * scale
    );
    // console.log("scene y:" + viewer.scene.y);
    // console.log("scene x:" + viewer.scene.x);
    // console.log("scene scx:" + viewer.scene.scaleX);
    // console.log("scene scy:" + viewer.scene.scaleY);
    // console.log("scene mouseX:" + viewer.scene.mouseX);
    // console.log("scene mouseY:" + viewer.scene.mouseY);
    // console.log("width of body:" + viewer.width);

    // 首次渲染地图居中
    if (!check) {
      viewer.scene.x -= (gridClient.currentGrid.pose.position.x * viewer.scene.scaleX);
      viewer.scene.y += (gridClient.currentGrid.pose.position.y * viewer.scene.scaleY);
      check = true;
    }

    callback(self);
  });

  // 添加拖拽事件
  var oldX = 0;
  var oldY = 0;

  viewer.scene.on("mousedown", function (evt) {
    oldX = evt.stageX;
    oldY = evt.stageY;
  });

  viewer.scene.on("pressmove", function (evt) {
    evt.currentTarget.x += evt.stageX - oldX;
    evt.currentTarget.y += evt.stageY - oldY;
    oldX = evt.stageX;
    oldY = evt.stageY;
  });

  // 添加机器人坐标
  drawRobotMarker();
}

/**
 * 根据坐标为当前地图添加机器人本体的箭头
 */
function drawRobotMarker() {
  var rootObject = viewer.scene;
  // get a handle to the stage
  var stage;
  if (rootObject instanceof window.CREATEJS.Stage) {
    stage = rootObject;
  } else {
    stage = rootObject.getStage();
  }

  // marker for the robot
  var robotMarker = new window.ROS2D.NavigationArrow({
    size: 25,
    strokeSize: 1,
    fillColor: window.CREATEJS.Graphics.getRGB(255, 128, 0, 0.66),
    pulse: false
  });

  // wait for a pose to come in first
  robotMarker.visible = false;
  rootObject.addChild(robotMarker);
  // var initScaleSet = false;

  // setup a listener for the robot pose
  var poseListener = new ROSLIB.Topic({
    ros: ros,
    name: '/robot_pose',
    messageType: 'geometry_msgs/Pose',
    throttle_rate: 100
  });
  
  poseListener.subscribe(function (pose) {
    // update the robots position on the map
    robotMarker.x = pose.position.x;
    robotMarker.y = -pose.position.y;

    console.log(stage.scaleX);
    console.log(stage.scaleY);

    // 机器人箭头的比例随舞台大小变化而变化
    robotMarker.scaleX = 1.0 / stage.scaleX;
    robotMarker.scaleY = 1.0 / stage.scaleY;

    // if (!initScaleSet) {
    //   robotMarker.scaleX = 1.0 / stage.scaleX;
    //   robotMarker.scaleY = 1.0 / stage.scaleY;
    //   initScaleSet = true;
    // }

    // change the angle
    robotMarker.rotation = stage.rosQuaternionToGlobalTheta(pose.orientation);

    robotMarker.visible = true;
  });
}

/**
 * 修改地图比例
 * @param {number} newValue 不能小于零
 */
function changeScale(newValue) {
  if (newValue > 1 || newValue <= 0) {
    console.log("input illegal scale value" + newValue);
  } else {
    scale = newValue;
    try {
      viewer.scaleToDimensions(gridClient.currentGrid.width * scale, gridClient.currentGrid.height * scale);
    } catch {
      console.log("scale changed error,maybe the viewer is not existed");
    }

  }
}

/**
 * 为当前地图添加网格线，默认居中
 * @param {number} size 整个的网格的大小
 * @param {number} cellSize 网格内的单格大小
 * @param {number} lineWidth 网格线的粗细
 */
function addGird(size = 10, cellSize = 1, lineWidth = 0.005) {
  grid = new window.ROS2D.Grid({
    size: size,
    cellSize: cellSize,
    lineWidth: lineWidth
  });
  gridClient.rootObject.addChild(grid);
}

/**
 * 移除当前地图的网格
 */
function removeGrid() {
  gridClient.rootObject.removeChild(grid);
}

/**
 * 完全置空所有连接、主题和地图
 */
function destroy() {
  try {
    ros.close();
  } catch {
    console.log("ros close error in drawjs");
  }

  ros = null;
  viewer = null;
  gridClient = null;
  // mapsTopic.unadvertise();
  check = false;
}

export { createView, destroy, changeScale, draw, viewer, addGird, removeGrid };
